import SimpleOpenNI.*;
import processing.opengl.*;
//import pbox2d.*;
import org.jbox2d.common.*;
import org.jbox2d.callbacks.*;
import org.jbox2d.collision.shapes.*;
import org.jbox2d.dynamics.*;
import org.jbox2d.dynamics.contacts.*;

SimpleOpenNI kinect;

PFont font;
PImage pigImg, birdImg, woodImg, stoneImg;
PImage backImg, midImg, frontImg;
PImage poseImg;
Layer backLayer, midLayer;
PBox2D box2d;
Camera cam;
Surface surface;
Slingshot slingshot;
Pig pig;
int pigs;
ArrayList<Bird> birds;
ArrayList<Block> blocks;
float MIN_DAMAGE = 250;
float MAX_CAMERA_Y = 600;
float MIN_CAMERA_X = 320;
float MAX_CAMERA_X = 960;

boolean isDrawn;
boolean isReleased;
boolean isReadyRelease;
boolean isReady;
boolean isFlightEnd;

int state;

void setup() {
  size(640, 480, P3D);
  
  kinect = new SimpleOpenNI(this);
  kinect.setMirror(true);
  if (kinect.enableDepth() == false) {
     println("Can't open the depthMap, maybe the camera is not connected!");
     exit();
     return;
  }
  kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
  
  background(255);
  rectMode(CENTER);
  imageMode(CENTER);
  textMode(SCREEN);
  smooth();
  font = createFont("FEASFBI_.TTF", 28);
  textFont(font);
  backImg = loadImage("back.png");
  midImg = loadImage("mid.png");
  pigImg = loadImage("pig.png");
  frontImg = loadImage("front.png");
  poseImg = loadImage("pose.png");
  
  box2d = new PBox2D(this, width * 2, height * 2);
  box2d.createWorld();
  box2d.listenForCollisions();
  cam = new Camera(width * 1.5, MAX_CAMERA_Y);
  backLayer = new Layer(width, height * 0.75, 1.5, backImg, cam);
  midLayer = new Layer(width, height * 1.4, 2, midImg, cam);
  surface = new Surface(width, height * 1.75, width * 2, height * 0.5, frontImg);
  slingshot = new Slingshot(160, 660);
  pig = new Pig(slingshot.ZERO_POS.x, slingshot.ZERO_POS.y);
  pig.body.setAwake(false);
  pigs = 3;
  slingshot.load(pig);
  birds = new ArrayList<Bird>();
  birds.add(new Bird(996, 654));
  birds.add(new Bird(996, 594));
  birds.add(new Bird(996, 534));
  blocks = new ArrayList<Block>();
  blocks.add(new Stone(960, 696));
  blocks.add(new Stone(1032, 696));
  blocks.add(new Wood(996, 666));
  blocks.add(new Stone(960, 636));
  blocks.add(new Stone(1032, 636));
  blocks.add(new Wood(996, 606));
  blocks.add(new Stone(960, 576));
  blocks.add(new Stone(1032, 576));
  blocks.add(new Wood(996, 546));
  blocks.add(new Stone(960, 516));
  blocks.add(new Stone(1032, 516));
  blocks.add(new Wood(996, 486));
  isDrawn = false;
  isReleased = false;
  isReady = false;
  isReadyRelease = false;
  isHandOpen = true;
  state = 0;
}

LinkedList<PVector> handVectorBuffer = new LinkedList<PVector>();
int HAND_VECTOR_BUFFER_SIZE = 5;
PVector leftHandVector, rightHandVector;
int handOpenCount = 0;
int handClosedCount = 0;
int HAND_STATE_CHANGE_COUNT = 5;
float HAND_VECTOR_CLOUD_DISTANCE = 150;
float HAND_OPEN_DISTANCE = 75;
int STEPS = 2;
boolean isHandOpen;
float lastHeadZ;
float currentHeadZ;

void addHandVectorBuffer(PVector v) {
  handVectorBuffer.add(v);
  if (handVectorBuffer.size() > HAND_VECTOR_BUFFER_SIZE) {
    handVectorBuffer.remove();
  }
}

PVector getHandVectorBufferAverage() {
  PVector sum = new PVector();
  for (PVector v : handVectorBuffer) {
    sum.add(v);
  }
  sum.div((float)handVectorBuffer.size());
  return sum;
}

// for testing
//int count = 60;

void draw() {
  if (!isReleased) {
    kinect.update();
    IntVector userList = new IntVector();
    kinect.getUsers(userList);
    if (isReady && userList.size() > 0) {
      int userId = userList.get(0); 
      if (kinect.isTrackingSkeleton(userId)) {  
        int[] depthMap = kinect.depthMap();
        int index;
        PVector realWorldPoint;
        ArrayList<PVector> handVectorCloud = new ArrayList<PVector>();
        leftHandVector = getLeftHandVector(userId);
        rightHandVector = getRightHandVector(userId);
        lastHeadZ = currentHeadZ;
        currentHeadZ = getHeadVector(userId).z;
        // populate handVectorCloud
        for (int y = 0; y < kinect.depthHeight(); y += STEPS) {
          for (int x = 0; x < kinect.depthWidth(); x += STEPS) {
            index = x + y * kinect.depthWidth();
            if (depthMap[index] > 0) {
              realWorldPoint = kinect.depthMapRealWorld()[index];
              float d1 = leftHandVector.dist(realWorldPoint);
              float d2 = rightHandVector.dist(realWorldPoint);
              if (d1 < HAND_VECTOR_CLOUD_DISTANCE && d2 > HAND_VECTOR_CLOUD_DISTANCE) {
                handVectorCloud.add(realWorldPoint);
              }
            }
          } 
        }
        // check and update hand state
        boolean isLeftHandOpen = false;
        for (PVector v1 : handVectorCloud) {
          if (v1.x < leftHandVector.x - HAND_OPEN_DISTANCE) {
            for (PVector v2 : handVectorCloud) {
              if (v2.x > leftHandVector.x + HAND_OPEN_DISTANCE) {
                isLeftHandOpen = true;
                break;
              }
            }
          }
          if (v1.y < leftHandVector.y - HAND_OPEN_DISTANCE) {
            for (PVector v2 : handVectorCloud) {
              if (v2.y > leftHandVector.y + HAND_OPEN_DISTANCE) {
                isLeftHandOpen = true;
                break;
              }
            }
          }
          if (isLeftHandOpen) break;
        }
        if (isLeftHandOpen) {
          ++handOpenCount;
          handClosedCount = 0;
        }
        else {
          ++handClosedCount;
          handOpenCount = 0;
        }
        if (handOpenCount >= HAND_STATE_CHANGE_COUNT) {
          isHandOpen = true;
          handOpenCount = 0;
        }
        else if (handClosedCount >= HAND_STATE_CHANGE_COUNT) {
          isHandOpen = false;
          isReadyRelease = true;
          handClosedCount = 0;
        }
      }
    }
  }
  
  background(255);
  box2d.step();
  // for testing
  /*if (count > 0) {
    if (--count == 0) {
      state = 1;
      println("state 1");
    }
  }*/
  switch (state) {
    case 0:
      break;
    case 1:
      state1();
      break;
    case 2:
      //state2();
      break;
    case 3:
      state3();
      break;
    case 4:
      //state4();
      break;
    default:
      break;
  }
  cam.constrainCamera();
  cam.apply();
  // drawing
  backLayer.draw();
  midLayer.draw();
  Iterator iter = blocks.iterator();
  while (iter.hasNext ()) {
    Block block = (Block)iter.next();
    block.draw();
    if (block.isOutOfHealth()) {
      block.destroy();
      iter.remove();
    }
  }
  iter = birds.iterator();
  while (iter.hasNext ()) {
    Bird bird = (Bird)iter.next();
    bird.draw();
    Vec2 pos = box2d.getBodyPixelCoord(bird.body);
    if (bird.isOutOfHealth() || pos.y > cam.MAX_H) {
      bird.destroy();
      iter.remove();
    }
  }
  slingshot.drawBack();
  if (pig != null) pig.draw();
  slingshot.drawFront();
  surface.draw();
  fill(0);
  text("pigs: " + pigs, 10, 30);
  text("birds: " + birds.size(), width - 100, 30);
  if (state == 0) {
    text("raise your arms to start", 190, 80);
    image(poseImg, cam.x, cam.y - 60, 90, 157);
  }
  else if (state == 4) {
    state4();
  }
}

void drawSkeleton(int userId) {  
  stroke(0); 
  strokeWeight(5);  
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HAND, SimpleOpenNI.SKEL_RIGHT_HAND); 
  noStroke();   
  fill(255, 255, 0); 
  drawJoint(userId, SimpleOpenNI.SKEL_RIGHT_HAND);
  drawJoint(userId, SimpleOpenNI.SKEL_LEFT_HAND);
} 
PVector getRightHandVector(int userId) {
  PVector rightHand = new PVector();
  kinect.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rightHand);
  return rightHand;
}
PVector getLeftHandVector(int userId) {
  PVector leftHand = new PVector();
  kinect.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, leftHand);
  return leftHand;
}
PVector getHeadVector(int userId) {
  PVector head = new PVector();
  kinect.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, head);
  return head;
}
float getAngle(int userId) {
  PVector rightHand = new PVector();
  PVector leftHand = new PVector();
  kinect.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, leftHand);
  kinect.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rightHand);
  //PVector r =new PVector(rightHand.x,rightHand.y);
  //PVector l = new PVector(leftHand.x,leftHand.y);
  PVector mid =leftHand.cross(rightHand);
  //println(leftHand);
  //println(rightHand);
  //println(mid);
  PVector horizon = new PVector(rightHand.x,0);
  float angleInRadians = PVector.angleBetween(mid,horizon);
  return(angleInRadians);
  //println(angleInRadians, 10,10);
}
float getJointDistance(int userId, int jointID1, int jointID2) {  
  PVector joint1 = new PVector();  
  PVector joint2 = new PVector();  
  kinect.getJointPositionSkeleton(userId, jointID1, joint1); 
  kinect.getJointPositionSkeleton(userId, jointID2, joint2);  
  return PVector.dist(joint1, joint2);
} 

void drawJoint(int userId, int jointID) { 
  PVector joint = new PVector();  
  float confidence = kinect.getJointPositionSkeleton(userId, jointID, joint); 
  if (confidence < 0.5) {
    return;  
  }
  // convert the detected hand position 
  // to "projective" coordinates 
  // that will match the depth image
  PVector convertedJoint = new PVector(); 
  kinect.convertRealWorldToProjective(joint, convertedJoint);
  ellipse(convertedJoint.x, convertedJoint.y, 5, 5);
}

// user-tracking callbacks!  
void onNewUser(int userId) {
  println("start pose detection");
  kinect.startPoseDetection("Psi", userId); 
}  

void onStartPose(String pose, int userId) {
  println("Started pose for user");
  kinect.stopPoseDetection(userId);
  kinect.requestCalibrationSkeleton(userId, true);
}

void onEndCalibration(int userId, boolean successful) {  
  if (successful) {
    println(" User calibrated !!!"); 
    kinect.startTrackingSkeleton(userId);
    state = 1;
    println("state 1");
  }
  else {
    println(" Failed to calibrate user !!!");
    kinect.startPoseDetection("Psi", userId);
  } 
}

// camera transition back to slingshot
void state1() {
  if (cam.s > cam.MIN_SCALE) {
    cam.scaleCamera(cam.s * 0.99);
  }
  if (cam.x > MIN_CAMERA_X || cam.y != MAX_CAMERA_Y) {
    Vec2 camPos = new Vec2(cam.x, cam.y);
    Vec2 targetPos = new Vec2(MIN_CAMERA_X, MAX_CAMERA_Y);
    targetPos.subLocal(camPos);
    if (targetPos.length() < 1) {
      cam.translateCamera(MIN_CAMERA_X, MAX_CAMERA_Y);
    }
    else {
      targetPos.mulLocal(0.1);
      cam.translateCamera(cam.x + targetPos.x, cam.y + targetPos.y);
    }
    if (!isReady && slingshot.ZERO_POS.x > cam.x - cam.halfW * 1.5) {
      isReady = true;
      println("state 2");
    }
  }
  else {
    //state = 2;
    //println("state 2");
  }
  if (isReady) {
    state2();
  }
}

// ready to shoot
void state2() {
  if (isReadyRelease && isHandOpen) {
    slingshot.release();
    isReleased = true;
    isFlightEnd = false;
    isReady = false;
    isReadyRelease = false;
    state = 3;
    println("state 3");
  }
  else if (isReady && leftHandVector != null && rightHandVector != null) {
    PVector d = PVector.sub(leftHandVector, rightHandVector);
    d.y = -d.y;
    d.mult(0.1);
    d.add(new PVector(slingshot.ZERO_POS.x, slingshot.ZERO_POS.y));
    Vec2 pos = slingshot.constrainPos(d.x, d.y);
    pig.body.setTransform(box2d.coordPixelsToWorld(pos), 0);
    /*float dHeadZ = currentHeadZ - lastHeadZ;
    if (abs(dHeadZ) > 100) {
      if (dHeadZ < 0) {
        cam.scaleCamera(cam.s * 1.01 * (int)(-dHeadZ / 100));
      }
      else if (dHeadZ > 0) {
        cam.scaleCamera(cam.s * 0.99 * (int)(dHeadZ / 100));
        cam.translateCamera(slingshot.x, MAX_CAMERA_Y);
      }
    }*/
  }
}

// in flight
void state3() {
  if (cam.s < cam.MAX_SCALE && cam.x + cam.halfW == cam.MAX_W) {
    isFlightEnd = true;
  }
  Vec2 pos = box2d.getBodyPixelCoord(pig.body);
  Vec2 vel = pig.body.getLinearVelocity();
  float scaleThreshold = cam.y - cam.h * 0.25;
  if (!isFlightEnd && vel.y > 0 && pos.y < scaleThreshold) {
    cam.scaleCamera(cam.s * 1.01);
  }
  if (cam.s > cam.MIN_SCALE && vel.y < 0 && pos.y > scaleThreshold) {
    cam.scaleCamera(cam.s * 0.99);
    cam.y += pos.y - scaleThreshold;
    cam.x = pos.x;
  }
  if (!isFlightEnd) {
    if (pos.x > cam.x) {
      cam.x = pos.x;
    }
  }
  else {
    cam.x = cam.MAX_W - cam.halfW;
  }
  if (cam.y > MAX_CAMERA_Y) {
    cam.y = MAX_CAMERA_Y;
  }
  // check for continue condition
  if (!pig.body.isAwake() || pos.y > cam.MAX_H) {
    --pigs;
    pig.destroy();
    if (pigs == 0 || birds.size() == 0) {
      pig = null;
      state = 4;
      println("state 4");
    }
    else {
      pig = new Pig(slingshot.ZERO_POS.x, slingshot.ZERO_POS.y);
      pig.body.setAwake(false);
      slingshot.load(pig);
      state = 1;
      isDrawn = false;
      isReleased = false;
      println("state 1");
    }
  }
}

// game ends
void state4() {
  frameRate(0);
  fill(0, 0, 0, 32);
  rect(width, height, cam.MAX_W, cam.MAX_H);
  fill(0);
  if (birds.size() == 0) {
    text("level cleared!", width * 0.5 - 80, height * 0.5);
  }
  else {
    text("level failed!", width * 0.5 - 80, height * 0.5);
  }
}

void postSolve(Contact c, ContactImpulse ci) {
  // Get both shapes
  Fixture f1 = c.getFixtureA();
  Fixture f2 = c.getFixtureB();
  // Get both bodies
  Body b1 = f1.getBody();
  Body b2 = f2.getBody();
  // Get our objects that reference these bodies
  Object o1 = b1.getUserData();
  Object o2 = b2.getUserData();
  float sum = 0;
  /*if (o1.getClass() == Pig.class && o2.getClass() == Surface.class) {
   for (float i : ci.normalImpulses) {
   sum += i;
   }
   }
   if (o1.getClass() == Surface.class && o2.getClass() == Pig.class) {
   for (float i : ci.normalImpulses) {
   sum += i;
   }
   }*/
  if (o1.getClass().getSuperclass() == Block.class && o2.getClass() == Surface.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Block)o1).health -= sum;
    }
  }
  if (o1.getClass() == Surface.class && o2.getClass().getSuperclass() == Block.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Block)o2).health -= sum;
    }
  }
  if (o1.getClass().getSuperclass() == Block.class && o2.getClass() == Pig.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Block)o1).health -= sum;
    }
  }
  if (o1.getClass() == Pig.class && o2.getClass().getSuperclass() == Block.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Block)o2).health -= sum;
    }
  }
  if (o1.getClass().getSuperclass() == Block.class && o2.getClass() == Bird.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Block)o1).health -= sum;
      ((Bird)o2).health -= sum * 2;
    }
  }
  if (o1.getClass() == Bird.class && o2.getClass().getSuperclass() == Block.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Bird)o1).health -= sum * 2;
      ((Block)o2).health -= sum;
    }
  }
  if (o1.getClass() == Pig.class && o2.getClass() == Bird.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Bird)o2).health -= sum * 2;
    }
  }
  if (o1.getClass() == Bird.class && o2.getClass() == Pig.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Bird)o1).health -= sum * 2;
    }
  }
  if (o1.getClass().getSuperclass() == Block.class && o2.getClass().getSuperclass() == Block.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    if (sum > MIN_DAMAGE) {
      ((Block)o1).health -= sum * 0.5;
      ((Block)o2).health -= sum * 0.5;
    }
  }
  // throws exceptions for unknown reason
  /*if (o1.getClass() == Bird.class && o2.getClass() == Surface.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    sum *= 2;
    if (sum > DAMAGE_LOWER_BOUND) {
      ((Block)o1).health -= sum;
    }
  }
  if (o1.getClass() == Surface.class && o2.getClass() == Bird.class) {
    for (float i : ci.normalImpulses) {
      sum += i;
    }
    sum *= 2;
    if (sum > DAMAGE_LOWER_BOUND) {
      ((Block)o2).health -= sum;
    }
  }*/
  if (sum > MIN_DAMAGE) println("impulse: " + sum);
}

// mouse and keyboard callbacks for testing
void mousePressed() {
  float x = cam.x + (mouseX - width * 0.5) * cam.s;
  float y = cam.y + (mouseY - height * 0.5) * cam.s;
  if (isReady && pig.contains(x, y)) {
    isDrawn = true;
  }
}

void mouseDragged() {
  float x = cam.x + (mouseX - width * 0.5) * cam.s;
  float y = cam.y + (mouseY - height * 0.5) * cam.s;
  if (isDrawn && !isReleased) {
    Vec2 position = slingshot.constrainPos(x, y);
    pig.body.setTransform(box2d.coordPixelsToWorld(position), 0);
  }
}

void mouseReleased() {
  if (isDrawn && !isReleased) {
    slingshot.release();
    isReleased = true;
    isFlightEnd = false;
    isReady = false;
    isReadyRelease = false;
    isHandOpen = true;
    state = 3;
    println("state 3");
  }
}

void keyPressed() {
  switch(key) {
  case 'w':
    cam.translateCamera(cam.x, cam.y - 5);
    break;
  case 's':
    cam.translateCamera(cam.x, cam.y + 5);
    break;
  case 'a':
    cam.translateCamera(cam.x - 5, cam.y);
    break;
  case 'd':
    cam.translateCamera(cam.x + 5, cam.y);
    break;
  case 'z':
    cam.scaleCamera(cam.s * 0.9);
    break;
  case 'x':
    cam.scaleCamera(cam.s * 1.1);
    break;
  default:
    break;
  }
}

